
#include <Metro.h> //Include Metro library

const byte ingasgriff = 7; //Analog
const byte outgasgriff = 9; //PWM

byte aktOut = 0;

//Timer
Metro MreadGas(100);

void setup()
{
  initPins();
  pinMode(outgasgriff, OUTPUT);
  Serial.begin(9600);
}

void loop()
{
  if (MreadGas.check()){
    int in = analogRead(ingasgriff);
    Serial.print("in ");Serial.print(in);
    byte out = calcGaspwm(in);
    Serial.print(" : out ");Serial.println(out);
    if (out > wantOut){
      wantOut := (byte)smooth(in, 0.08, wantOut);
    }
    else{
      wantOut := (byte)smooth(in, 0.3, wantOut);
    }
    Serial.print("wantout = ");Serial.println(wantOut); 
  }
}

void initPins(){
	for (int i=2; i<13;i++)
		pinMode(i,INPUT);
}

const int ingasgriffval[15]={176,180,188,225,266,323,395,457,522,610,673,725,732,735,737};
const byte outgasgriffval[15]={13,27,40,54,68,81,95,108,122,136,149,163,176,190,204};

byte calcGaspwm(const int aInval){
  //index suchen
  byte Idx;
  int x1, x2 = 0;
  for (Idx=0; Idx<15; Idx++) {
    x1 = ingasgriffval[Idx];
    if(Idx<14) {x2 = ingasgriffval[Idx+1];}
    else {x2 = 1024;} 
    
    if(inRange(aInval, x1, x2))
    	break;
  }
  return outgasgriffval[Idx]; 
}

bool inRange(const int aVal, const int aMin, const int aMax)
{
	if((aVal>=aMin) && (aVal<aMax)){
    Serial.print(aVal);Serial.print(": >= ");Serial.print(aMin);Serial.print(" < ");Serial.println(aMax);
    return true;
  } 
  else return false;
}

int smooth(int inVal, float filterVal, float smoothedValVal){
  smoothedVal = (inVal * (1 - filterVal)) + (smoothedVal  *  filterVal);

  return (int)smoothedVal;
}
